#include "widget.h"
#include "ui_widget.h"
#include <QDebug>
#include <QMouseEvent>
#include<QPaintEvent>
#include<QPainter>
#include <vector>
#include<Eigen/Core>
#include<Eigen/Dense>

using namespace std;


const double eps = 1e-3;

using veci = std::vector<int >;
using vecd = std::vector<double >;


template<class T >
T pp(T x){
    return x * x;
}

template<class T >
T ppp(T x){
    return x * x * x;
}


vector<double> ChordalParameterization(vecd &x, vecd &y) {
    double tot = 0;
    int n=x.size();
    for(int i=1;i<n;++i) {
        double a = x[i]-x[i-1];
        double b = y[i]-y[i-1];
        tot += sqrt(a*a+b*b);
    }

    vector<double> t(n, 0);
    double cur = 0;
    for(int i=1;i<n-1;++i) {
        double a = x[i]-x[i-1];
        double b = y[i]-y[i-1];
        cur += sqrt(a*a+b*b);
        t[i]=cur/tot;
    }

    t[n-1]=1;
    return t;
}


struct B3{

  vector<double > M, h; // [0, n - 1]
  vecd x, y; // [0, n]
  int p = 0;

  // 三弯矩求解三次样条
  void getB3(){
    ///////////////////////////////////////////////////////////////
    // 利用 x y 求出三次样条，把系数保存到 this -> M,h 中

    int n = x.size()-1;
    M.resize(n+1, 0);
    h.resize(n+1, 0);

    vector<double> b(n);
    for(int i=0;i<n;++i) {
        h[i]=x[i+1]-x[i];
        b[i] = 6./h[i]*(y[i+1]-y[i]);
    }
    p=0;

    if(n-1<=0)return;

    Eigen::MatrixXd A(n-1, n-1);

    Eigen::VectorXd v(n-1);

    for(int i=0;i<n-1;++i) {
        v(i) = b[i+1]-b[i];
        A(i,i) = 2*(h[i+1]+h[i]);
        if(i-1>=0)A(i, i-1) = h[i];
        if(i+1<n-1)A(i, i+1)=h[i+1];
    }

    // 追赶法求解
    for(int i=0;i<n-2;++i) {
        double q = A(i+1, i)/A(i, i);
        A(i+1, i)=0;
        A(i+1,i+1) -= A(i, i+1)*q;
        v[i+1] -= v[i]*q;
    }

    for(int i=n-2;i>=0;--i) {
        M[i+1] = v[i]/A(i,i);
        if(i==0)break;
        v[i-1]-=M[i+1]*A(i-1,i);
        A(i-1,i)=0;
    }
  }

  double get(double t){
    if(x.size()<2)return -1000000;
    if(t<x[0] || t>x.back())return -1000000;
    while(p+1 < x.size() && t > x[p + 1]){
      p++;
    }

    return M[p]/(6*h[p])*ppp(x[p+1]-t) + M[p+1]/(6*h[p])*ppp(t-x[p])
            + (y[p+1]/h[p]-M[p+1]*h[p]/6)*(t-x[p]) + (y[p]/h[p]-M[p]*h[p]/6)*(x[p+1]-t);
  }

  double getGradient(double t){
    if(x.size()<2)return -1000000;
    if(t<x[0] || t>x.back())return -1000000;
    while(p+1 < x.size() && t > x[p + 1]){
      p++;
    }

    return -M[p]/(2*h[p])*pp(x[p+1]-t) + M[p+1]/(2*h[p])*pp(t-x[p])
            + (y[p+1]/h[p]-M[p+1]*h[p]/6) - (y[p]/h[p]-M[p]*h[p]/6);
  }
};



Widget::Widget(QWidget *parent)
    : QWidget(parent)
    , ui(new Ui::Widget)
{
    ui->setupUi(this);
}


void Widget::mouseReleaseEvent(QMouseEvent *event) {
    qDebug()<<"mouseRelease";
  if(event->button() == Qt::RightButton){
      // 初始化生成曲线
       initSpline();
  }
  else if(event->button() == Qt::LeftButton){

    QPoint clickPosition = event->pos();
    int clickX = clickPosition.x();
    int clickY = clickPosition.y();
    ControlPoint* controlt = new ControlPoint(this);
    controlt->setMiddle(clickX, clickY);
    cp.push_back(controlt);
    controlt->show();
  }else if(event->button()==Qt::MidButton){
      for(int i=0;i<cp.size();++i) delete(cp[i]);
      cp.clear();
      update();
  }
}

void Widget::mouseDoubleClickEvent(QMouseEvent *event)
{
}

bool Widget::eventFilter(QObject *watched, QEvent *event)  {
//qDebug()<<event->type();
           if (event->type() == QEvent::Move) {
               hermite();
           } else if (event->type() == QEvent::Resize) {
               hermite();
           }
       return QObject::eventFilter(watched, event);
}

void Widget::paintEvent(QPaintEvent *event){
  QWidget::paintEvent(event);
  QPainter painter(this);

  // 绘制图例
  painter.setPen(Qt::green);
  painter.drawLine(10, 10, 50, 10);
  painter.setPen(Qt::black);
  painter.drawText(60, 15, "三次样条");


  painter.setPen(Qt::green);
  while(!lines.empty()) {
      auto f=lines.front();
      lines.pop();
      //qDebug()<<f.first.x()<<f.first.y();
      painter.drawLine(f.first, f.second);
  }
}


// 初始化三次样条
void Widget::initSpline()
{
    if(cp.size()<2)return;
    B3 bsx;
    B3 bsy;
    vector<double> t, x,y;


    for(int i=0;i<cp.size();++i) {
        auto m = cp[i]->getMiddle();
        x.push_back(m.x());
        y.push_back(m.y());
    }

    t=ChordalParameterization(x, y);

    bsx.x=t;
    bsx.y=x;
    bsx.getB3();
    bsy.x=t;
    bsy.y=y;
    bsy.getB3();
    QPoint pre;
    bool first=true;
    for(double i=t[0];i<t.back();i+=0.01) {
        QPoint cur(bsx.get(i), bsy.get(i));
        if(!first)lines.push({pre, cur});
        pre=cur;
        first=false;
    }    
    // 计算导数和更新点信息
    bsx.p=0;
    bsy.p=0;
    for(int i=0;i<t.size();++i) {
        float tx = bsx.getGradient(t[i])/10;
        float ty = bsy.getGradient(t[i])/10;
        cp[i]->setRight(tx, ty);
        cp[i]->installEventFilter(this);
    }

    update();
}

void Widget::hermite()
{
    qDebug()<<"hermite";
    if(cp.size()<2)return;
    bool first=true;
    Eigen::Vector2f pre(cp[0]->getMiddle().x(), cp[0]->getMiddle().y());
    for(int i=0;i<cp.size()-1;++i) {
        Eigen::Vector2f p0(cp[i]->getMiddle().x(), cp[i]->getMiddle().y());
        Eigen::Vector2f p1(cp[i+1]->getMiddle().x(), cp[i+1]->getMiddle().y());

        float delta = 3;
        Eigen::Vector2f d0(cp[i]->getRight().x()*delta, cp[i]->getRight().y()*delta);
        Eigen::Vector2f d1(-cp[i+1]->getLeft().x()*delta, -cp[i+1]->getLeft().y()*delta);
        for(double t=0;t<1;t+=0.01) {
            Eigen::Vector2f P = p0*(2*ppp(t)-3*pp(t)+1) + p1*(-2*ppp(t)+3*pp(t))
                    +d0*(ppp(t)-2*pp(t)+t)+d1*(ppp(t)-pp(t));
            if(!first)lines.push({QPoint(pre.x(), pre.y()), QPoint(P.x(), P.y())});
            first=false;
            pre=P;
        }
    }
    qDebug()<<lines.size();

    update();
}


Widget::~Widget()
{
    delete ui;
}

